package cn.nexd.app.arsenal;


public class StepDetector {
	
	ButtLPFilter filter = new ButtLPFilter();
	
	final int THL = 4;
	final float THH = 0.11f;

	int st=1;
	int ct=0;
	int pt=1;
	float xst = 0;
	float xpt = 0;
	int flag=0;
	int trend=0;
	int index = 0;
	float lastValue = 0;
	
	private int stepCount = 0;
	
	public int getStepCount() {
		return stepCount;
	}

	public StepDetector()  {
		clear();
	}
	
	public void clear() {
		st = 1;
		ct = 0;
		pt = 1;
		xst = 0;
		xpt = 0;
		flag = 0;
		trend = 0;
		index = 0;
		lastValue = 0;
		
		stepCount = 0;
		
		filter.clear();
	}

	public int detect(float input) {
		
		// LP Filter
		input = filter.filter(input);
		
		index=index+1;
	    if(index==1) {
	        trend=(int) Math.signum(input - lastValue);
	        lastValue = input;
	        return 0;
		}
	    int tt=(int) Math.signum(input - lastValue);
	    if(tt==trend) {
	        lastValue = input;
	    	return 0;
	    }
	    trend=tt;
	    if(flag==0) {
	    	st=pt;
	    	pt=index;
	    	xst = xpt;
	    	xpt = input;
	    } else {
	    	pt=index;
	    	flag=0;
	    	xpt = input;
	    }
	    if(pt-st<THL || Math.abs(xpt - xst)<THH) {
	    	flag=1;
	        lastValue = input;
	    	return 0;
	    }

	    if(ct==2){
	    	ct=1;
	        lastValue = input;
	        stepCount++;
	    	return 1;
	    } else {
	    	ct=ct+1;
	    }
        lastValue = input;
	    return 0;
	}
	
	public class ButtLPFilter {
		final static int BUFFER_LENGTH = 20;
		double[] xbuffer = new double[BUFFER_LENGTH];
		double[] ybuffer = new double[BUFFER_LENGTH];
		/*
		 * PARAM 3
		double[] B_Arr = {	1.000000000000000,
				   -2.788055939605452,
				    3.046470778186613,
				   -1.523350025713034,
				    0.292289551535585};
		double[] A_Arr = {  0.001709647775232,
			    0.006838591100928,
			    0.010257886651392,
			    0.006838591100928,
			    0.001709647775232};
		*/
		
		// * PARAM 1
//		double[] A_Arr = {0.000298940539714,
//				0.001793643238285,   
//				0.004484108095713,   
//				0.005978810794282,
//				0.004484108095712,   
//				0.001793643238285,   
//				0.000298940539714
//				};
//		double[] B_Arr = {1.000000000000000,  
//				   -3.639852443812422,
//				    5.828361659135576,  
//				   -5.169523378660825,
//				    2.657879849024126,  
//				   -0.747097954410503,   
//				    0.089364463265755};
//		
		 
		/*
		 * PARAM 2
		double[] A_Arr = {0.001150742544106,
				0.005753712720530,
				0.011507425441061,   
				0.011507425441061,
				0.005753712720530,
				0.001150742544106

				};
		double[] B_Arr = {		1.000000000000000,
				   -3.025837663462862,
				    3.917198222080807,  
				   -2.645651055885413,
				    0.923532450543623,
				   -0.132418191864761};
				   */
		
//		double[] B_Arr={1,-11.0222097446940,56.6665859844020,-180.066544205202,395.020334279893,-632.741536869534,763.028117458715,-703.608035215701,498.479689323329,-269.969729510025,110.011600084360,-32.7040806407390,6.70397775709147,-0.848123531717302,0.0499548833483934};
//		double[] A_Arr = {3.26702553898883e-12,4.57383575458437e-11,2.97299324047984e-10,1.18919729619194e-09,3.27029256452782e-09,6.54058512905564e-09,9.81087769358346e-09,1.12124316498097e-08,9.81087769358346e-09,6.54058512905564e-09,3.27029256452782e-09,1.18919729619194e-09,2.97299324047984e-10,4.57383575458437e-11,3.26702553898883e-12};
//		double[] A_Arr = {2.24334439913321e-13,3.36501659869981e-12,2.35551161908987e-11,1.02072170160561e-10,3.06216510481683e-10,6.73676323059702e-10,1.12279387176617e-09,1.44359212084222e-09,1.44359212084222e-09,1.12279387176617e-09,6.73676323059702e-10,3.06216510481683e-10,1.02072170160561e-10,2.35551161908987e-11,3.36501659869981e-12,2.24334439913321e-13};
//		double[] B_Arr = {1,-11.9891091427451,67.3300948679838,-234.915568267038,569.381652123658,-1015.37296113253,1376.08926902244,-1443.05902815935,1180.45513742372,-753.178101390691,371.729248358900,-139.356584941186,38.4094636308401,-7.34719906065887,0.872107810850588,-0.0484211368422843};
		 double A_Arr[] = {
			        2.24334439913321E-013D, 3.36501659869981E-012D, 2.3555116190898699E-011D, 1.0207217016056101E-010D, 3.0621651048168301E-010D, 6.7367632305970204E-010D, 1.1227938717661701E-009D, 1.4435921208422199E-009D, 1.4435921208422199E-009D, 1.1227938717661701E-009D, 
			        6.7367632305970204E-010D, 3.0621651048168301E-010D, 1.0207217016056101E-010D, 2.3555116190898699E-011D, 3.36501659869981E-012D, 2.24334439913321E-013D
			    };
			    double B_Arr[] = {
			        1.0D, -11.9891091427451D, 67.330094867983803D, -234.915568267038D, 569.381652123658D, -1015.37296113253D, 1376.08926902244D, -1443.05902815935D, 1180.4551374237201D, -753.17810139069104D, 
			        371.72924835890001D, -139.35658494118599D, 38.409463630840101D, -7.3471990606588697D, 0.87210781085058797D, -0.048421136842284301D
			    };

		int count = 0;
		
		public ButtLPFilter() {
			clear();
		}
		
		public void clear() {
			count = 0;
			for (int i = 0 ; i < BUFFER_LENGTH ; i++) {
				xbuffer[i] = 0;
				ybuffer[i] = 0;
			}
		}
		
		private void shift (double[] arr) {
			for (int i = 0; i < arr.length-1 ; i++) {
				arr[i] = arr[i+1];
			}
		}
		
		public float filter(float input) {
			double w=0,v=0;
			int k;
			shift(xbuffer);
			shift(ybuffer);
			xbuffer[xbuffer.length-1] = input;
			count++;
			for (k=0;k<A_Arr.length;k++){
				if (count - k > 0) {
					v=(double) (v+A_Arr[k]*(double)xbuffer[xbuffer.length-1-k]);
				}
			}
			for (k=1;k<B_Arr.length;k++) {
				if (count-k>0) {
					w=(double) (w+B_Arr[k]*(double)ybuffer[ybuffer.length-1-k]);
				}
			}
			ybuffer[ybuffer.length-1]=(double) ((-w+v)/B_Arr[0]);
			return (float) ybuffer[ybuffer.length-1];
		}
	}
}